#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Empty.h>

using namespace std;

serial::Serial ser;         //声明串口对象


//回调函数
//～～     void write_callback(const std_msgs::UInt8::ConstPtr& msg)
//～～     有ConstPtr 写msg->data                
void write_callback(const std_msgs::UInt8& msg)     
{
    cout<<"sending"<<endl;
    ROS_INFO_STREAM("Writing to serial port"<<msg.data);
    // 发送串口数据
    char test[3];
    test[0] = 0x55;
    test[1] = 0xAA;
    test[2] = msg.data;
    cout<<msg.data<<endl;
    ser.write(test);
}
 
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// string tohex(const string s)    //字符串转16位
// {
//     while (1)
//     {
//         int n = 0;
//         // 将字符串转换为int类型
//         for (int i = s.length() - 1, j = 1; i >= 0; i--)
//          {
//             n += (s[i] - '0') * j;
//             j *= 10;
//         }
//         // 运用短除法不断除以16取余数，并将其加入字符串结果中
//         string result = "";
//         char _16[] = 
//         {
//             '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'
//         };
//         // 16进制，除以16
//         const int radix = 16;
//         while (n)
//          {
//             int i = n % radix;          // 余数
//             result = _16[i] + result;   // 将余数对应的十六进制数字加入结果
//             n /= radix;                 // 除以16获得商，最为下一轮的被除数
//         }
//         return result;
//     }
// }
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~


int main (int argc, char **argv)
{
    ros::init(argc,argv,"motor_move");              //初始化节点
    ros::NodeHandle nh;                                          //声明节点句柄
    //订阅话题并配置回调函数
     
    ros::Subscriber write_sub = nh.subscribe("/mode",1000,write_callback);           //  回调函数
    //发布话题
    try
    {
        //设置串口属性，打开串口

        //配置串口

        ser.setPort("/dev/ttyUSB0");               
        //配置波特率
        ser.setBaudrate(115200);
        //timeout: 读写超时时间
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        //打开串口
        ser.open();
    }
    catch(serial::IOException& e)
    {
       ROS_ERROR_STREAM("Unable to open port");
       return -1;
    }

//检测串口是否打开，并给出提示信息
    if (ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }

    //循环频率
    ros::Rate loop_rate(5);
    
    while(ros::ok())
    {
        int value;
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}